#define DTY 20
#define DTX 40

#include <time.h>
#include <unistd.h>
#include <iostream>
#include <stdlib.h>
#include <stdio.h>
#define random(x) (rand()%x)

using namespace std;
struct point {
	int x, y;
};
/*
A*算法在每个节点中加入了路径的信息:
F:当前点的路径信息，包含了到当前点到起点与到终点的信息。（F=G+H）
G:当前点到唯一的起点的路径信息。
H:当前点到指定终点的距离信息。
*/
struct A_node {
	// 路径节点
	struct point pt; // 当前节点坐标
	int g, h, f; // 评估值
	struct point fpt; // 父节点坐标
	struct A_node *next; // 链表下一个节点
};

struct bu_node {
	int fx; // 运动方向
	struct bu_node *next;
};

class Astar {
public:
	struct A_node *openlist; // 开启列表
	struct A_node *closelist; // 关闭列表
	struct bu_node *bulist; // 运动步序列表
	struct point spt; // 开始点坐标
	struct point ept;//目标点坐标
	int DT[DTY][DTX]; // 一张二维数组表示的地图（根据实际需求设定大小）

	Astar(); // 初始化
	void setspt(int y, int x); // 设置开始坐标
	void setept(int y, int x); // 设置目标坐标
	void setdt(); // 设置地图信息0为通路
	void addopennode(struct A_node *node); // 增加一个开启节点
	void delopennode(struct A_node *node); // 删除一个开启节点
	void addclosenode(struct A_node *node); // 增加一个关闭节点
	int inopenlist(struct A_node *node); // 判断节点是否已经在开启列表中
	int incloselist(struct A_node *node); // 判断节点是否已经在关闭列表中
	void pinggu(struct A_node *node, int G); // 对地图上的一个节点进行价值评估
	void axgo(); // 开始一步寻路
	int find(); // 是否找到路径判定
	void gofind(); // 开始寻路
	void print();
	void addbu(int fx); // 往列表添加一步
	int delbu(); // 从列表删除并返回一步
};

Astar::Astar() 
{
	openlist = NULL;
	closelist = NULL;
	bulist = NULL;
}

void Astar::setspt(int y, int x) 
{
	spt.y = y;
	spt.x = x;
	// 设置并把开始节点添加到开启列表
	openlist = (A_node *) malloc(sizeof(A_node));
	if (openlist == NULL)
	{
		printf("开启列表创建失败！");
	}
	else
		cout<<"开启列表创建成功！";
	openlist->pt = spt;
	openlist->g = 1;
	openlist->next = NULL;
}

void Astar::setept(int y, int x)
 {
	ept.y = y;
	ept.x = x;
}

void Astar::setdt() {
	srand((int)time(0));	
	for (int y = 0; y <= 21; y++)
	{
		for (int x = 0; x <= 41; x++)
		{
			if(y==0|x==0|y==21|x==41)
			{
				DT[y][x]=1;
			}
			else
			{
				int i=random(50);
				if(i<=1)

				{
					DT[y][x]=1;
					
				}
				else
				{
					DT[y][x]=0;
				}	
			}
				if(DT[y][x]==0)
					cout<<" ";
				else
					cout<<"#";
		}			
				cout<<endl;	
			
		}
	}	
	//DT[y][x] = dt[y][x];

void Astar::pinggu(struct A_node *node, int G) 
{
	if (inopenlist(node))
	{
		node->f = 0;
		node->g = 0;
		node->h = 0;
	} 
	else if (incloselist(node))
	{
		node->f = 0;
		node->g = 0;
		node->h = 0;
	} 
	else if (DT[node->pt.y][node->pt.x] == 8)
	{
		//cout<<"DT[node->pt.y][node->pt.x]"<<DT[node->pt.y][node->pt.x]<<endl;
		node->f = 0;
		node->g = 0; // 障碍标记这里为贪吃蛇身标记
		node->h = 0;
	} 
	else 
	{
		node->g = G + 10;
		int H, H1;
		H = node->pt.y - ept.y;
		cout<<"node->pt.y"<<"ept.y"<<node->pt.y<<"  "<<ept.y<<"H"<<H<<endl;
		if (H < 0)
		H = ept.y - node->pt.y;
		H1 = node->pt.x - ept.x;
		if (H1 < 0)
		H1 = ept.x - node->pt.x;
		node->h = (H + H1) * 10;
		node->f = node->h + node->g;
		cout<<"node->f"<<node->f<<endl;
		
	}

}

void Astar::addopennode(struct A_node *node) {
struct A_node *p;
p = (A_node *) malloc(sizeof(A_node));
*p = *node;
p->next = openlist;
//cout<<"open readly!!"<<endl;
openlist = p;
}

void Astar::delopennode(struct A_node *node) 
{
	struct A_node *p;
	p = openlist;
	if (openlist != NULL)
	openlist = openlist->next;
	if (p != NULL)
	{
		*node = *p;
		free(p);
	}
}

void Astar::addclosenode(struct A_node *node) 
{
	struct A_node *p;
	p = (A_node *) malloc(sizeof(A_node));
	*p = *node;
	p->next = closelist;
	closelist = p;
}

void Astar::addbu(int fx)
{
	struct bu_node *p;
	p = (bu_node *) malloc(sizeof(bu_node));
	p->fx = fx;
	p->next = bulist;
	bulist = p;
}

int Astar::delbu() 
{
	struct bu_node *p;
	int fx = 0;
	p = bulist;
	if (bulist != NULL)
	bulist = bulist->next;
	if (p != NULL) 
	{
		fx = p->fx;
		free(p);
	}
	return fx;
}

int Astar::inopenlist(struct A_node *node)
{
	struct A_node *p;
	p = openlist;
	while (p != NULL) 
	{
		if ((p->pt.y == node->pt.y) && (p->pt.x == node->pt.x))
		return 1;
		p = p->next;
	}
	return 0; // 在返回1不在返回0
}

int Astar::incloselist(struct A_node *node) 
{
	struct A_node *p;
	p = closelist;
	while (p != NULL) 
	{
		if ((p->pt.y == node->pt.y) && (p->pt.x == node->pt.x))
		return 1;
		p = p->next;
	}
	return 0; // 在返回1不在返回0
}

int Astar::find() 
{
	struct A_node *p;
	p = closelist;
	cout<<"go to"<<endl;
	while (p != NULL) 
	{
		if ((p->pt.y == ept.y) && (p->pt.x == ept.x))
		return 1;
		p = p->next;
	}
	return 0; // 在返回1不在返回0
}

void Astar::axgo() 
{
	struct A_node p, p1, p2, p3, p4; // 不斜着走所以只对四个方向进行评估
	delopennode(&p); // 从开启列表取出最上层的节点
	addclosenode(&p); // 把这个节点加入到关闭列表
	p1.pt.y = p.pt.y + 1; // 上面的节点
	//cout<<"p1.pt.y"<<p1.pt.y<<endl;
	p1.pt.x = p.pt.x;
	p2.pt.y = p.pt.y - 1; // 下面的节点
	p2.pt.x = p.pt.x;
	p3.pt.y = p.pt.y; // 左面的节点
	p3.pt.x = p.pt.x - 1;
	p4.pt.y = p.pt.y; // 右面的节点
	p4.pt.x = p.pt.x + 1;
	p1.fpt = p.pt; // 设置父节点坐标
	p2.fpt = p.pt;
	p3.fpt = p.pt;
	p4.fpt = p.pt;
	p1.g = p1.h = p1.f = 0;
	p2.g = p2.h = p2.f = 0;
	p3.g = p3.h = p3.f = 0;
	p4.g = p4.h = p4.f = 0;
	//cout<<"go to there"<<endl;
	if ((p1.pt.y >= 0) && (p1.pt.y < DTY) && (p1.pt.x >= 0) && (p1.pt.x < DTX))
	pinggu(&p1, p.g); // 对当前节点相邻的节点进行价值评估

	if ((p2.pt.y >= 0) && (p2.pt.y < DTY) && (p2.pt.x >= 0) && (p2.pt.x < DTX))
	pinggu(&p2, p.g);

	if ((p3.pt.y >= 0) && (p3.pt.y < DTY) && (p3.pt.x >= 0) && (p3.pt.x < DTX))
	pinggu(&p3, p.g);

	if ((p4.pt.y >= 0) && (p4.pt.y < DTY) && (p4.pt.x >= 0) && (p4.pt.x < DTX))
	pinggu(&p4, p.g);

	int bp1, bp2, bp3, bp4;
	bp1 = bp2 = bp3 = bp4 = 1; // 排序用入栈标记
	
	// 4个待选节点排序后入栈
	for (int i = 0; i < 4; i++) 
	{
		if ((p1.f >= p2.f * bp2) && (p1.f >= p3.f * bp3) && (p1.f >= p4.f * bp4)&& (bp1 == 1)) 
		{
			if ((p1.f + p1.g + p1.h) > 0)
				addopennode(&p1);
				bp1 = 0;
		}
		if ((p2.f >= p1.f * bp1) && (p2.f >= p3.f * bp3) && (p2.f >= p4.f * bp4)&& (bp2 == 1)) 
		{
			if ((p2.f + p2.g + p2.h) > 0)
				addopennode(&p2);
				bp2 = 0;
		}
		if ((p3.f >= p1.f * bp1) && (p3.f >= p2.f * bp2) && (p3.f >= p4.f * bp4)&& (bp3 == 1)) 
		{
			if ((p3.f + p3.g + p3.h) > 0)
				addopennode(&p3);
				bp3 = 0;
		}
		if ((p4.f >= p1.f * bp1) && (p4.f >= p2.f * bp2) && (p4.f >= p3.f * bp3)&& (bp4 == 1)) 
		{
			if ((p4.f + p4.g + p4.h) > 0)
				addopennode(&p4);
				bp4 = 0;
		}
	}
}

void Astar::gofind() 
{
	while ((openlist != NULL) && (!find()))
	axgo();
	if (openlist == NULL)
	printf("找不到可通行路径！\n");

	if (find()) 
	{
		
	// 生成运动方向列表
		struct A_node *p;
		int fx;
		p = closelist;
		while (p != NULL) 
		{
			if (p->fpt.y > p->pt.y)
			fx = 1;
			else // 上
			if (p->fpt.y < p->pt.y)
			fx = 2;
			else // 下
			if (p->fpt.x > p->pt.x)
			fx = 3;
			else // 左
			if (p->fpt.x < p->pt.x)
			fx = 4; // 右
			addbu(fx);
			p = p->next;
		}
	}
}

void Astar::print() 
{
	printf("开始打印开启列表：\n");
	struct A_node *p;
	p = openlist;
	while (p != NULL) 
	{
		printf("节点坐标：%d %d\n", p->pt.y, p->pt.x);
		printf("父节点坐标：%d %d\n", p->fpt.y, p->fpt.x);
		printf("评估值：%d %d %d\n\n", p->g, p->h, p->f);
		p = p->next;
	}
	printf("\n开始打印关闭列表：\n");
	p = closelist;
	while (p != NULL) 
	{
		printf("节点坐标：%d %d\n", p->pt.y, p->pt.x);
		printf("父节点坐标：%d %d\n", p->fpt.y, p->fpt.x);
		printf("评估值：%d %d %d\n\n", p->g, p->h, p->f);
		p = p->next;
	}
}
int main() 
{
	Astar A;
	A.setdt();
	//while (1) 
	//{

		sleep(1);
		
		// 初始化A星算法
		A.setspt(1,1);
		A.setept(20, 40);
		A.gofind();
		//A.print();//打印开启和关闭列表信息

		A.delbu(); // 去掉起点信息

	//}
	return 0;
}